import 机械臂正逆解
import numpy as np
a1,b1,c1,d1,e1,f1=650,0,450,0,0,1
a2,b2,c2=-120,-120,35.77
a3,b3,c3=120,-120,35.77
a4,b4,c4=-110,50,31.86
a5,b5,c5=70,40,19.92

a,b,c,d,e,f=机械臂正逆解.inverse_kinematics(600,0,600,0,0,1)


def createtable():
    TEST=[]
    TEST1=[]
    r1=机械臂正逆解.inverse_kinematics(a1+a2,b1+b2,c1+c2,d1,e1,f1)
    for r in r1:
        TEST1.append(r)
    TEST.append(TEST1.copy())
    TEST1.clear()
    r2=机械臂正逆解.inverse_kinematics(a1+a3,b1+b3,c1+c3,d1,e1,f1)
    for r in r2:
        TEST1.append(r)
    TEST.append(TEST1.copy())
    TEST1.clear()
    r3=机械臂正逆解.inverse_kinematics(a1+a4,b1+b4,c1+c4,d1,e1,f1)
    for r in r3:
        TEST1.append(r)
    TEST.append(TEST1.copy())
    TEST1.clear()
    r4=机械臂正逆解.inverse_kinematics(a1+a5,b1+b5,c1+c5,d1,e1,f1)
    for r in r4:
        TEST1.append(r)
    TEST.append(TEST1.copy())
    TEST1.clear()
    return  TEST

output=createtable()
for i in output:
    print(i)
# print("c=",c,"  ",type(c),"  ")
# print("a1,b1,c1,d1,e1,f1=",机械臂正逆解.inverse_kinematics(a1+a2,b1+b2,c1+c2,d1,e1,f1))
# print("a1,b1,c1,d1,e1,f1=",机械臂正逆解.inverse_kinematics(a1+a3,b1+b3,c1+c3,d1,e1,f1))
# print("a1,b1,c1,d1,e1,f1=",机械臂正逆解.inverse_kinematics(a1+a4,b1+b4,c1+c4,d1,e1,f1))
# print("a1,b1,c1,d1,e1,f1=",机械臂正逆解.inverse_kinematics(a1+a5,b1+b5,c1+c5,d1,e1,f1))
